
public class RushHourHeuristic2 implements Heuristic {

	RushHourCar.Orientation ourOrientation;//doesn't change
	int ourHeadCol;//changes
	int ourHeadRow;//changes
	int ourLength;//doesn't change
	int minMustBeClear;//changes
	int maxMustBeClear;//changes
	int exitCol;//doesn't change
	int exitRow;//doesn't change
	
	public RushHourHeuristic2() {
		
	}
	
	public void init(State initialState) {
		RushHourState s = (RushHourState)initialState;
		RushHourCar ourCar = s.cars.get(0);
		ourOrientation = ourCar.getOrientation();
		ourLength = ourCar.getLength();
		exitCol = s.exitPos.getCol();
		exitRow = s.exitPos.getRow();
	}
	
	public int getHeuristic(State state) {
		callCounter++;
        int cost = 0;
		RushHourState s = (RushHourState)state;
		RushHourCar ourCar = s.cars.get(0);
		ourHeadCol = ourCar.getHead().getCol();
		
		//find the grids between our car and the exit
		//head is top/left of car
		if(ourOrientation.equals(RushHourCar.Orientation.HORIZONTAL)) {
			if(exitCol>ourHeadCol) {
				//exit to the right
				minMustBeClear =  ourHeadCol+ourLength;
				maxMustBeClear = exitCol-1;
			}
			else {
				//exit to the left
				minMustBeClear = 1;
				maxMustBeClear = ourHeadCol-1;
			}
		}
		else {
			if(exitRow>ourHeadRow) {
				//exit down
				minMustBeClear =  ourHeadRow+ourLength;
				maxMustBeClear = exitRow-1;
			}
			else {
				//exit up
				minMustBeClear = 1;
				maxMustBeClear = ourHeadRow-1;
			}			
		}
				
        //for each car that might be in the way, calculate the cost of moving it out of our car's way
        for(int i=1; i<s.cars.size(); i++) {
        	cost += getMovingCost(s, s.cars.get(i));
        } 

        //add the cost of moving our car from its current position to the exit
		if(ourOrientation.equals(RushHourCar.Orientation.HORIZONTAL)) {
			if(exitCol>ourHeadCol) {
				cost += exitCol-(ourHeadCol+ourLength)+1;
			}
			else {
				cost += ourHeadCol;
			}
		}
		else {
			if(exitRow>ourHeadRow) {
				cost += exitRow-(ourHeadRow+ourLength)+1;
			}
			else {
				cost += ourHeadRow;
			}			
		}                
        return ++cost;//++ because the goal is to be in the exit grid, not only next to it		
	}
	
	int getMovingCost(RushHourState s, RushHourCar car) {
		
		int cost = 0;		
		RushHourCar ourCar = s.cars.get(0);		
		if(!car.getOrientation().equals(ourOrientation)) {
			//car must have different orientation to be able to move out of the way
			if(car.getOrientation().equals(RushHourCar.Orientation.HORIZONTAL)) {
				int carRow = car.getHead().getRow();
				if(carRow>=minMustBeClear && carRow<=maxMustBeClear) {
					//the car's row is within the range that our car has to go to the exit
					int carMinCol = car.getHead().getCol();
					int carMaxCol = carMinCol + car.getLength()-1;			
					int ourHeadCol = ourCar.getHead().getCol();
					if(carMinCol<=ourHeadCol && carMaxCol>=ourHeadCol) {
						//a part of the car is in the same column as our car
						//can it move left and what does it cost?
						int leftCost = Integer.MAX_VALUE;
						int leftSpace = carMinCol-1;						
						int rightOffset = carMaxCol-ourHeadCol+1;
						if(leftSpace>=rightOffset) {
							leftCost = rightOffset;
						}
						
						//can it move right and what does it cost?
						int rightCost = Integer.MAX_VALUE;
						int rightSpace = s.grid.getHeight()-(carMaxCol+1)-1;						
						int leftOffset = ourHeadCol-carMinCol+1;
						if(rightSpace>=leftOffset) {
							rightCost = leftOffset;
						}
						cost += Math.min(leftCost, rightCost)+1;
					}
				}
			}
			else {
				int carCol = car.getHead().getCol();
				if(carCol>=minMustBeClear && carCol<=maxMustBeClear) {				
					//the car's column is within the range that our car has to go to the exit
					int carMinRow = car.getHead().getRow();
					int carMaxRow = carMinRow + car.getLength()-1;			
					int ourHeadRow = ourCar.getHead().getRow();
					if(carMinRow<=ourHeadRow && carMaxRow>=ourHeadRow) {
						//a part of the car is in the same row as our car
						//can it move up and what does it cost?
						int upCost = Integer.MAX_VALUE;
						int upSpace = carMinRow-1;						
						int downOffset = carMaxRow-ourHeadRow+1;
						if(upSpace>=downOffset) {
							upCost = downOffset;
						}
						
						//can it move down and what does it cost?
						int downCost = Integer.MAX_VALUE;
						int downSpace = s.grid.getHeight()-(carMaxRow+1)-1;						
						int upOffset = ourHeadRow-carMinRow+1;
						if(downSpace>=upOffset) {
							downCost = upOffset;
						}
						cost += Math.min(upCost, downCost)+1;	
					}
				}
			}
		}		
		return cost;
	}	
	
	public long callCounter = 0;
	public long getCallCount(){return callCounter;}

}
